#include "qrviz.hpp"
#include <QVBoxLayout>
#include <QTimer>
#include <rviz/view_manager.h>
#include <QFile>
#include <QDebug>
#include <QMessageBox>
#include <QVector3D>

QRviz::QRviz(QWidget *parent):QWidget(parent) {

  setContentsMargins(0,0,0,0);
  //创建rviz容器
  render_panel_ = new rviz::RenderPanel(this);
  render_panel_->setCursor(Qt::PointingHandCursor);

  //向layout添加widget
  QVBoxLayout *mainLayout= new QVBoxLayout(this);
  mainLayout->addWidget(render_panel_);


  //初始化rviz控制对象
  manager_ = new rviz::VisualizationManager(render_panel_);
  if(!manager_)
  {
    qDebug() << "manager_ err";
  }
  ROS_ASSERT(manager_ != NULL);
  //获取当前rviz控制对象的 tool控制对象
  tool_manager_ = manager_->getToolManager();
  ROS_ASSERT(tool_manager_ != NULL);
  //初始化camera 这行代码实现放大 缩小 平移等操作

  render_panel_->initialize(manager_->getSceneManager(), manager_);
  render_panel_->setBackgroundColor(Ogre::ColourValue(.188,.188,.188));
  view_manager_ = manager_->getViewManager();
  view_manager_->setRenderPanel(render_panel_);
  manager_->initialize();
  tool_manager_->initialize();
  manager_->removeAllDisplays();
  manager_->startUpdate();
  manager_->setFixedFrame("camera_init");
}

void QRviz::Show() { render_panel_->show(); }
void QRviz::Hide() { render_panel_->hide(); }
rviz::Display* RobotModel_ = NULL;
//显示robotModel
void QRviz::Display_RobotModel(bool enable) {
  if (RobotModel_ == NULL) {
    RobotModel_ =
        manager_->createDisplay("rviz/RobotModel", "Qrviz RobotModel", enable);
  } else {
    delete RobotModel_;
    RobotModel_ =
        manager_->createDisplay("rviz/RobotModel", "Qrviz RobotModel", enable);
  }
}

void QRviz::addSubNode(rviz::Property** pro, YAML::Node &node)
{
    for(YAML::const_iterator it= node.begin(); it != node.end();++it)
    {
       auto type = it->second.Type();
       QString key = QString::fromStdString(it->first.as<std::string>());
       auto subProp = (*pro)->subProp(key);
       if(type == YAML::NodeType::Scalar)
       {
           QString val = QString::fromStdString(it->second.as<std::string>());

           if(key == "Class" || key == "Name")
           {
               continue;
           }

           qInfo() << "rviz add key:" <<key << " value:" << val;
           subProp->setValue(val);

       }else if(type == YAML::NodeType::Map){
           qInfo() << "rviz  add sub prob:" << key;
           auto subNode = it->second.as<YAML::Node>();
           addSubNode(&subProp, subNode);
       }
    }
}

void QRviz::addDisPlay(YAML::Node &node)
{
    auto display = node["Displays"];
    if(display.IsNull() || !display.IsSequence())
    {
       qWarning() <<"rivz Displays is null or is not Sequence ";
       return;
    }

    for(YAML::const_iterator it= display.begin(); it != display.end(); ++it)
    {

        auto node  =*it;
        qDebug() << "node type:" << node.Type();


        if(!node["Class"].IsDefined() ||!node["Name"].IsDefined())
        {
            continue;
        }


        std::string tmp = node["Class"].as<std::string>();
        QString className =  QString::fromStdString(tmp);

        tmp = node["Name"].as<std::string>();
        QString name =  QString::fromStdString(tmp);

        bool Enabled = true;
        if(node["Enabled"].IsDefined())
        {
            Enabled = node["Enabled"].as<std::string>() == "true";
        }

        if(className == "rviz/Group")
        {
            qInfo()<< "addDisPlay Class:" << className << " name:" << name;
            addDisPlay(node);
            continue;
        }

        rviz::Display *rvizDisplay  = manager_->createDisplay(className, name,Enabled);
        qInfo()<< "createDisplay Class:" << className << " name:" << name << "Enabled:" << Enabled;
        addSubNode(&rvizDisplay,node);
    }
}

void QRviz::addSubNode(rviz::Display **pro, YAML::Node &node)
{
    for(YAML::const_iterator it= node.begin(); it != node.end();++it)
    {
       auto type = it->second.Type();
       QString key = QString::fromStdString(it->first.as<std::string>());
       auto subProp = (*pro)->subProp(key);
       if(type == YAML::NodeType::Scalar)
       {
           QString val = QString::fromStdString(it->second.as<std::string>());

           if(key == "Class" || key == "Name")
           {
               continue;
           }

           qInfo() << "rviz add key:" <<key << " value:" << val;
           subProp->setValue(val);

       }else if(type == YAML::NodeType::Map){
           qInfo() << "rviz add sub prob:" << key;
           auto subNode = it->second.as<YAML::Node>();
           addSubNode(&subProp, subNode);
       }
    }
}
//显示grid
void QRviz::Display_Grid(bool enable, QString Reference_frame,
                          QColor color) {
  if (grid_ == NULL) {
    grid_ = manager_->createDisplay("rviz/Grid", "adjustable grid", true);
    ROS_ASSERT(grid_ != NULL);
    // Configure the GridDisplay the way we like it.
    grid_->subProp("Line Style")->setValue("Billboards");
    grid_->subProp("Color")->setValue(color);
    grid_->subProp("Reference Frame")->setValue(Reference_frame);
    grid_->subProp("Alpha")->setValue(0.5);

  }

  grid_->setEnabled(enable);
  manager_->startUpdate();
}

void QRviz::Display_Axes(bool enable, QString Reference_frame, QString length, QString radius)
{
    if(!Axes_){
        Axes_ = manager_->createDisplay("rviz/Axes","map",true);
        Axes_->subProp( "Length")->setValue( length);
        Axes_->subProp( "Reference Frame" )->setValue(Reference_frame);
        Axes_->subProp( "Radius" )->setValue(radius);
    }

    Axes_->setEnabled(enable);
    manager_->startUpdate();
}

//显示map
void QRviz::Display_Map(bool enable, QString topic, double Alpha,
                        QString Color_Scheme) {

    auto map_ = manager_->createDisplay("rviz/Map", "QMap", enable);
    ROS_ASSERT(map_);
    map_->subProp("Topic")->setValue(topic);
    map_->subProp("Alpha")->setValue(Alpha);
    map_->subProp("Color Scheme")->setValue(Color_Scheme);


    if(const auto iter = m_topic.find(topic); iter != m_topic.end()) {
        delete iter->second;
    }

    m_topic[topic] = map_;

    map_->setEnabled(enable);
    manager_->startUpdate();
}


void QRviz::Display_Polygon(bool enable, QString topic) {
  if (polygon_ == NULL) {
    polygon_ = manager_->createDisplay("rviz/Polygon", "QPolygon", enable);
    ROS_ASSERT(polygon_);
    polygon_->subProp("Topic")->setValue(topic);
  } else {
    delete polygon_;
    polygon_ = manager_->createDisplay("rviz/Polygon", "QPolygon", enable);
    ROS_ASSERT(polygon_);
    polygon_->subProp("Topic")->setValue(topic);
  }
  polygon_->setEnabled(enable);
  manager_->startUpdate();
}

void QRviz::Display_Path(QString topic, QColor color, double lineWidth)
{
    auto p  = manager_->createDisplay("rviz/Path", topic, true);

    p->subProp("Topic")->setValue(topic);
    p->subProp( "Color" )->setValue(color);
    p->subProp( "Line Width" )->setValue(lineWidth);

    p->setEnabled(true);
   manager_->startUpdate();
}

void QRviz::Display_PoseWithCov(bool enable, QString name, QString topic,QColor color)
{
     auto  dis = manager_->createDisplay("rviz/PoseWithCovariance", name, enable);
     dis->subProp("Topic")->setValue(topic);
     dis->subProp( "Color" )->setValue(color);
     manager_->startUpdate();
}

void QRviz::Display_Marker(QString topic,QColor color)
{
    auto  dis = manager_->createDisplay("rviz/Marker", "Marker", true);
    dis->subProp("Topic")->setValue(topic);
    dis->subProp( "Color" )->setValue(color);
    manager_->startUpdate();
}

void QRviz::Set_Enable(QString topic, bool enable)
{
    if (const auto iter = m_topic.find(topic); iter != m_topic.end()) {
        iter->second->setEnabled(enable);
     }

    manager_->startUpdate();
}

void QRviz::Display_LeGoLoAMProject()
{
      manager_->setFixedFrame("camera_init");
    //TF
      rviz::Display* grid_ = manager_->createDisplay( "rviz/Grid", "Grid", true );
      grid_->subProp( "Cell Size")->setValue( "1");
      grid_->subProp( "Color" )->setValue(QColor(160,160,164));
      grid_->subProp("Reference Frame")->setValue("camera_init");


      rviz::Display *Axes_base=manager_->createDisplay("rviz/Axes","camera_init",true);
      Axes_base->subProp( "Length")->setValue( "0.7");
      Axes_base->subProp( "Reference Frame" )->setValue("camera_init");
      Axes_base->subProp( "Radius" )->setValue("0.1");




//     //diyiban
//    rviz::Display *map_=manager_->createDisplay("rviz/PointCloud2","Edge Features ",true);

//     map_->subProp("Topic")->setValue("/cloud_registered");
//     map_->subProp("Unreliable")->setValue("false");
//     map_->subProp("Selectable")->setValue("false");
//     map_->subProp("Style")->setValue("Points");
//     map_->subProp("Size (Pixels)")->setValue("3");
//     map_->subProp("Alpha")->setValue("1");
//     map_->subProp("Decay Time")->setValue("0.1");
//     map_->subProp("Color Transformer")->setValue("AxisColor");
//     map_->subProp("Position Transformer")->setValue("XYZ");
//      map_->subProp("Queue Size")->setValue("1");
//     map_->subProp("Axis")->setValue("Z");
//     map_->subProp("Channel Name")->setValue("intensity");

//     map_->subProp("Use Fixed Frame")->setValue("true");


//     map_->subProp("Autocompute Value Bounds")->subProp("Min Value")->setValue("-1.50121");
//     map_->subProp("Autocompute Value Bounds")->subProp("Max Value")->setValue("1.41766");
//     map_->subProp("Autocompute Value Bounds")->subProp("Value")->setValue("true");


    rviz::Display *lader_2=manager_->createDisplay("rviz/PointCloud2","surround",true);
    lader_2->subProp("Topic")->setValue("/cloud_registered");
    lader_2->subProp("Selectable")->setValue("false");
    lader_2->subProp("Style")->setValue("Points");
    lader_2->subProp("Alpha")->setValue("1");
    lader_2->subProp("Decay Time")->setValue("0.1");
    lader_2->subProp("Position Transformer")->setValue("XYZ");
    lader_2->subProp("Color Transformer")->setValue("AxisColor");
    lader_2->subProp("Queue Size")->setValue("1");
    lader_2->subProp("Use rainbow")->setValue("true");
    lader_2->subProp("Size (Pixels)")->setValue("3");
    lader_2->subProp("Use Fixed Frame")->setValue("true");


    lader_2->subProp("Max Color")->setValue(QColor(255,255,255));
//       lader_->subProp("Max Intensity")->setValue("96");
    lader_2->subProp("Min Color")->setValue(QColor(0,0,0));

//       lader_->subProp("Max Intensity")->setValue("4096");
//       lader_->subProp("Min Intensity")->setValue("0");

    rviz::Display *lader_1=manager_->createDisplay("rviz/PointCloud2","currPoints",true);
    lader_1->subProp("Topic")->setValue("/cloud_registered");
    lader_1->subProp("Selectable")->setValue("true");
    lader_1->subProp("Style")->setValue("Points");
    lader_1->subProp("Alpha")->setValue("0.15");
    lader_1->subProp("Decay Time")->setValue("1000");
    lader_1->subProp("Position Transformer")->setValue("XYZ");
    lader_1->subProp("Color Transformer")->setValue("AxisColor");
    lader_1->subProp("Queue Size")->setValue("100000");
    lader_1->subProp("Use rainbow")->setValue("true");
    lader_1->subProp("Use Fixed Frame")->setValue("true");


    lader_1->subProp("Size (Pixels)")->setValue("1");


    lader_1->subProp("Max Color")->setValue(QColor(255,255,255));
//       lader_->subProp("Max Intensity")->setValue("96");
    lader_1->subProp("Min Color")->setValue(QColor(0,0,0));

//       lader_->subProp("Max Intensity")->setValue("4096");
//       lader_->subProp("Min Intensity")->setValue("0");






//     rviz::Display *map2_=manager_->createDisplay("rviz/PointCloud2","Surface Features (pink)",true);

//      map2_->subProp("Topic")->setValue("/laser_cloud_less_flat");
//      map2_->subProp("Color")->setValue(QColor(255,170,255));
//      map2_->subProp("Style")->setValue("Points");
//      map2_->subProp("Size (Pixels)")->setValue("5");
//      map2_->subProp("Color Transformer")->setValue("FlatColor");
//      map2_->subProp("Queue Size")->setValue("2");
//      map2_->subProp("Alpha")->setValue("1");
//      map2_->subProp("Position Transformer")->setValue("XYZ");
//      map2_->subProp("Decay Time")->setValue("0");

//      map2_->subProp("Invert Rainbow")->setValue("false");
//      map2_->subProp("Max Color")->setValue(QColor(255,255,255));
//      map2_->subProp("Max Intensity")->setValue("96");
//      map2_->subProp("Min Color")->setValue(QColor(0,0,0));
//      map2_->subProp("Min Intensity")->setValue("1");


//      map2_->subProp("Size (m)")->setValue("0.01");
//      map2_->subProp("Use Fixed Frame")->setValue("true");
//      map2_->subProp("Use rainbow")->setValue("true");


//      map2_->subProp("Autocompute Intensity Bounds")->setValue("true");
//      map2_->subProp("Autocompute Value Bounds")->subProp("Max Value")->setValue("20.45");
//      map2_->subProp("Autocompute Value Bounds")->subProp("Min Value")->setValue("-3.58");
//      map2_->subProp("Autocompute Value Bounds")->subProp("Value")->setValue("true");

//      map2_->subProp("Size (m)")->setValue("0.01");
//      map2_->subProp("Use Fixed Frame")->setValue("true");
//      map2_->subProp("Use rainbow")->setValue("true");


//      rviz::Display *map3_=manager_->createDisplay("rviz/PointCloud2","pointCloud2",true);

//      map3_->subProp("Topic")->setValue("/registered_cloud");
//      map3_->subProp("Selectable")->setValue("true");
//      map3_->subProp("Style")->setValue("Points");
//      map3_->subProp("Size (Pixels)")->setValue("2");
//      map3_->subProp("Color Transformer")->setValue("AxisColor");
//      map3_->subProp("Queue Size")->setValue("10");
//      map3_->subProp("Position Transformer")->setValue("XYZ");
//      map3_->subProp("Alpha")->setValue("0.5");
//      map3_->subProp("Decay Time")->setValue("300");
//      map3_->subProp("Axis")->setValue("Y");
//      map3_->subProp("Queue Size")->setValue("10");
//      map3_->subProp("Autocompute Intensity Bounds")->setValue("true");





//       rviz::Display *map4_=manager_->createDisplay("rviz/PointCloud2","pointCloud2",true);

//        map4_->subProp("Topic")->setValue("/key_pose_origin");
//        map4_->subProp("Selectable")->setValue("true");
//        map4_->subProp("Style")->setValue("Flat Squares");
//        map4_->subProp("Size (m)")->setValue("0.3");
//        map4_->subProp("Size (Pixels)")->setValue("2");
//        map4_->subProp("Color Transformer")->setValue("AxisColor");
//        map4_->subProp("Queue Size")->setValue("10");
//        map4_->subProp("Position Transformer")->setValue("XYZ");
//        map4_->subProp("Alpha")->setValue("0.5");
//        map4_->subProp("Decay Time")->setValue("300");
//        map4_->subProp("Axis")->setValue("Y");
//        map4_->subProp("Channel Name")->setValue("intensity");
//        map4_->subProp("Use rainbow")->setValue("true");
//        map4_->subProp("Size (m)")->setValue("0.3");

//        map4_->subProp("Max Color")->setValue(QColor(255,255,255));
//        map4_->subProp("Max Intensity")->setValue("96");
//        map4_->subProp("Min Color")->setValue(QColor(0,0,0));

//        map4_->subProp("Max Intensity")->setValue("187");
//        map4_->subProp("Min Intensity")->setValue("0");
}

void QRviz::Display_Lader()
{
       manager_->setFixedFrame("/rslidar");
       rviz::Display *lader_=manager_->createDisplay("rviz/PointCloud2","rslidar_points",true);
       lader_->subProp("Topic")->setValue("/rslidar_points");
       lader_->subProp("Selectable")->setValue("true");
       lader_->subProp("Style")->setValue("Squares");
       lader_->subProp("Alpha")->setValue("0.5");
       lader_->subProp("Decay Time")->setValue("0");
       lader_->subProp("Position Transformer")->setValue("XYZ");
       lader_->subProp("Color Transformer")->setValue("intensity");
       lader_->subProp("Queue Size")->setValue("10");
       lader_->subProp("Use rainbow")->setValue("true");
       lader_->subProp("Channel Name")->setValue("intensity");

       lader_->subProp("Size (m)")->setValue("0.1");
       lader_->subProp("Size (Pixels)")->setValue("2");


       lader_->subProp("Max Color")->setValue(QColor(255,255,255));
//       lader_->subProp("Max Intensity")->setValue("96");
       lader_->subProp("Min Color")->setValue(QColor(0,0,0));

//       lader_->subProp("Max Intensity")->setValue("4096");
//       lader_->subProp("Min Intensity")->setValue("0");


}

void QRviz::DemoveAllDisplays()
{
    manager_->removeAllDisplays();
    manager_->startUpdate();
}

//解析rviz配置文件
void QRviz::Load(QString path)
{
    YAML::Node config;

    try{
        config = YAML::LoadFile(path.toStdString());
    }
    catch(YAML::BadFile &e) {
        qWarning() <<"read rivz config error! path:" <<path << " err:" << e.what();
        return;
    }

     auto mgr =  config["Visualization Manager"];
     if(mgr.IsNull())
     {
        qWarning() <<"rivz Visualization Manager is null:" <<path;
        return;
     }

     auto display = mgr["Displays"];
     if(display.IsNull() || !display.IsSequence())
     {
        qWarning() <<"rivz Displays is null or is not Sequence :" <<path;
        return;
     }

     addDisPlay(mgr);
}

void QRviz::Show_TopDownView()
{
       // 设置俯视正交视图

    view_manager_->setCurrentFrom(view_manager_->create("rviz/TopDownOrtho"));
    view_manager_->setRenderPanel(render_panel_);

    // 配置视图参数
    view_manager_->getCurrent()->subProp("Target Frame")->setValue("map");
    view_manager_->getCurrent()->subProp("Near Clip Distance")->setValue("0.01");
}

void QRviz::Zero()
{
    rviz::ViewController* current_view = view_manager_->getCurrent();
    current_view->lookAt(0, 0, 0);
    current_view->subProp("Angle")->setValue(0);
}

//显示激光雷达
void QRviz::Display_LaserScan(bool enable, QString topic) {
  if (laser_ == NULL) {
    laser_ = manager_->createDisplay("rviz/LaserScan", "QLaser", enable);
    ROS_ASSERT(laser_);
    laser_->subProp("Topic")->setValue(topic);
  } else {
    delete laser_;
    laser_ = manager_->createDisplay("rviz/LaserScan", "QLaser", enable);
    ROS_ASSERT(laser_);
    laser_->subProp("Topic")->setValue(topic);
  }
  laser_->setEnabled(enable);
  manager_->startUpdate();
}
//设置全局显示
void QRviz::SetGlobalOptions(QString frame_name, QColor backColor,
                             int frame_rate) {
  manager_->setFixedFrame(frame_name);
  manager_->setProperty("Background Color", backColor);
  manager_->setProperty("Frame Rate", frame_rate);
  manager_->startUpdate();
}

// "rviz/MoveCamera";
// "rviz/Interact";
// "rviz/Select";
// "rviz/SetInitialPose";
// "rviz/SetGoal";
//设置机器人导航初始位置
void QRviz::Set_Pos(const QString& topic) {

  //获取设置Pos的工具
  //添加工具
  //qDebug() << "numTools: " << tool_manager_->numTools() << endl;
  for(auto i =0; i < tool_manager_->numTools(); i++)
  {
     rviz::Tool* tool =  tool_manager_->getTool(i);
     //qDebug() <<i <<" tool->getName(): " << tool->getName();
     if(tool && tool->getName() == "2D Pose Estimate")
     {
        tool->getPropertyContainer()->subProp("Topic")->setValue(topic);
        tool_manager_->setCurrentTool(tool);
        manager_->startUpdate();
        return;
     }
  }

  current_tool = tool_manager_->addTool("rviz/SetInitialPose");
  //设置当前使用的工具为SetInitialPose（实现在地图上标点）

  tool_manager_->setCurrentTool(current_tool);
  manager_->startUpdate();

  //     tool_manager_->setCurrentTool()
}
//设置机器人导航目标点
void QRviz::Set_Goal() {
  //添加工具
  current_tool = tool_manager_->addTool("rviz/SetGoal");
  //设置goal的话题
  rviz::Property* pro = current_tool->getPropertyContainer();
  pro->subProp("Topic")->setValue("/move_base_simple/goal");
  //设置当前frame
  //manager_->setFixedFrame("map");
  //设置当前使用的工具为SetGoal（实现在地图上标点）
  tool_manager_->setCurrentTool(current_tool);

  manager_->startUpdate();
}
void QRviz::Set_MoveCamera() {
  //获取设置Pos的工具
  //添加工具

  current_tool = tool_manager_->addTool("rviz/MoveCamera");
  //设置当前使用的工具为SetInitialPose（实现在地图上标点）
  tool_manager_->setCurrentTool(current_tool);
  manager_->startUpdate();
}
void QRviz::Set_Select() {
  //获取设置Pos的工具
  //添加工具

  current_tool = tool_manager_->addTool("rviz/Select");
  //设置当前使用的工具为SetInitialPose（实现在地图上标点）
  tool_manager_->setCurrentTool(current_tool);
  manager_->startUpdate();
}
//显示tf坐标变换
void QRviz::Display_TF(bool enable) {
  if (TF_) {
    delete TF_;
    TF_ = NULL;
  }
  TF_ = manager_->createDisplay("rviz/TF", "QTF", enable);
}
//显示导航相关
void QRviz::Display_Navigate(bool enable, QString Global_topic,
                             QString Global_planner, QString Local_topic,
                             QString Local_planner) {
  if (Navigate_localmap) {
    delete Navigate_localmap;
    Navigate_localmap = NULL;
  }
  if (Navigate_localplanner) {
    delete Navigate_localplanner;
    Navigate_localplanner = NULL;
  }
  if (Navigate_globalmap) {
    delete Navigate_globalmap;
    Navigate_globalmap = NULL;
  }
  if (Navigate_globalplanner) {
    delete Navigate_globalplanner;
    Navigate_globalplanner = NULL;
  }
    QTimer::singleShot(1000,[=](){
        // local map
        Navigate_localmap = manager_->createDisplay("rviz/Map", "Qlocalmap", enable);
        Navigate_localmap->subProp("Topic")->setValue(Local_topic);
        Navigate_localmap->subProp("Color Scheme")->setValue("costmap");
    });
    QTimer::singleShot(2000,[=](){
        Navigate_localplanner =
            manager_->createDisplay("rviz/Path", "QlocalPath", enable);
        Navigate_localplanner->subProp("Topic")->setValue(Local_planner);
        Navigate_localplanner->subProp("Color")->setValue(QColor(0, 12, 255));
    });
    QTimer::singleShot(3000,[=](){
        // global map
        Navigate_globalmap =
            manager_->createDisplay("rviz/Map", "QGlobalmap", enable);
        Navigate_globalmap->subProp("Topic")->setValue(Global_topic);
        Navigate_globalmap->subProp("Color Scheme")->setValue("costmap");
    });
    QTimer::singleShot(4000,[=](){
        Navigate_globalplanner =
            manager_->createDisplay("rviz/Path", "QGlobalpath", enable);
        Navigate_globalplanner->subProp("Topic")->setValue(Global_planner);
        Navigate_globalplanner->subProp("Color")->setValue(QColor(255, 0, 0));
    });
  //更新画面显示
  manager_->startUpdate();
}


void QRviz::DisPlay_Point2(bool enable, QString topic)
{
    auto Point2_ = manager_->createDisplay("rviz/PointCloud2", "PointCloud2", enable);
    ROS_ASSERT(Point2_);
    Point2_->subProp( "Style" )->setValue( "Flat Squares" );
    Point2_->subProp("Topic")->setValue(topic);
    Point2_->subProp("Unreliable")->setValue("false");
    Point2_->subProp("Selectable")->setValue("true");
    Point2_->subProp("Style")->setValue("Squares");
    Point2_->subProp("Size (m)")->setValue("0.1");
    Point2_->subProp("Alpha")->setValue("1");
    Point2_->subProp("Decay Time")->setValue("0");
    Point2_->subProp("Position Transformer")->setValue("XYZ");//
    Point2_->subProp("Color Transformer")->setValue("Intensity");
    Point2_->subProp("Queue Size")->setValue("10");
    Point2_->subProp("Use rainbow")->setValue("true");
    Point2_->subProp("Invert Rainbow")->setValue("true");
    Point2_->subProp("Autocompute Intensity Bounds")->setValue("true");
    Point2_->subProp("Channel Name")->setValue("intensity");
    Point2_->setEnabled(enable);

    manager_->startUpdate();


    if (const auto iter = m_topic.find(topic); iter != m_topic.end()) {
        delete iter->second;
     }

    m_topic[topic] = Point2_;
}


void QRviz::createDisplay(QString display_name, QString topic_name) {}

